/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of class CUnitData
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "UnitData.hpp"
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Unit;
//*****************************************************************************
OWNDEBUG_INITIALIZE_MODULENUMBER(); // Initialize module print capability
//*****************************************************************************

CUnitData::CUnitData()
  : iTimestampPtr(NULL),
    iCommandPtr(NULL),
    iUnitIdPtr(NULL),
    iUnitPosePtr(NULL),
    iAvailableUnitArray(),
    iActiveUnitArray()
{
  SetLastElementType(KTypeUnknown);
}
//*****************************************************************************

CUnitData::~CUnitData()
{
  // Do any cleanups here.
}
//*****************************************************************************

CUnitData::CUnitData(const CUnitData &aData)
  :  iTimestampPtr(NULL),
     iCommandPtr(NULL),
     iUnitIdPtr(NULL),
     iUnitPosePtr(NULL),
     iAvailableUnitArray(),
     iActiveUnitArray()
{
  // Call assignment operator.
  *this = aData;
}
//*****************************************************************************

CUnitData &CUnitData::operator=(const CUnitData &aData)
{
  // Reset
  Reset();

  // Create an internal BinBag instance
  if (CreateInternalBinBag(aData.iBinBagContainer) == false) {
    dPrint(ODERROR,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aData);
  }
  
  // Do the decode on the buffer; it sets pointers and ... stuff :)
  if (DecodeFrom(NULL) == false) {
    dPrint(ODERROR,"Warning: Failed to Decode CUnitData even though assignment operator used?");
  }
  
  return *this; 
}
//*****************************************************************************

bool CUnitData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess. only overwrite if the
  // argument was non-NULL.
  if (aBinBag != NULL) 
    SetBinBagContainerPtr(aBinBag);

  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {

      case KTypeTimestamp:
        if (iTimestampPtr == NULL && 
            b->size == sizeof(Common::TTimestamp)) {
          iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

        } else result = false;
        break;

      case KTypeCommand: {
        if (iCommandPtr == NULL && 
            b->size == sizeof(TCommand)) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data); 

          
        } else result = false;
        break;
      }
        
      case KTypeUnitId: {
        if (iUnitIdPtr == NULL && 
            b->size == sizeof(TUnitId)) {

          iUnitIdPtr = reinterpret_cast<const TUnitId *>(b->data); 

          // Verify that the last character in the array is zero (Prevents buffer overflow)
          if (iUnitIdPtr->instance[sizeof(iUnitIdPtr->instance)-1] != '\0') {
            iUnitIdPtr = NULL;
            result = false;
          }

        } else result = false;
        break;
      }

      case KTypeUnitPose: {
        if (iUnitPosePtr == NULL && 
            b->size == sizeof(TUnitPose)) {
          
          iUnitPosePtr = reinterpret_cast<const TUnitPose *>(b->data); 
          
        } else result = false;
        break;
      }

      case KTypeAvailableUnit: {
        if (b->size == sizeof(TAvailableUnit)) {
          
          const TAvailableUnit *ptr = reinterpret_cast<const TAvailableUnit *>(b->data); 

          // Verify that the last character in the array is zero (Prevents buffer overflow)
          if (ptr->implementation[sizeof(ptr->implementation)-1] == '\0') {
            iAvailableUnitArray.push_back(ptr); 
          } else result = false;
        
        } else result = false;
        break;
      }
        
      case KTypeActiveUnit: {
        if (b->size == sizeof(TActiveUnit)) {
          
          const TActiveUnit *ptr = reinterpret_cast<const TActiveUnit *>(b->data); 
         
          // Verify that the last character in the array is zero (Prevents buffer overflow)
          if (ptr->instance[sizeof(ptr->instance)-1] == '\0') {
            iActiveUnitArray.push_back(ptr);
          } else result = false;          
          
          
        } else result = false;
        break;
      }
        
      default:
        // Unknown Type in BinBag. Do nothing.
        dPrint(ODWARN,"Unknown type %u, ignoring.", b->type);
        break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else result = false;

  // Store the result
  SetReadOnlyContainer(result);

  return result;
}
//*****************************************************************************

void CUnitData::Reset()
{
  // Call base class reset.
  CMaCIData::Reset();

  // Clean out. (Same values as in default constructor)
  iTimestampPtr = NULL;
  iCommandPtr = NULL;
  iUnitIdPtr = NULL;
  iUnitPosePtr = NULL;
  iAvailableUnitArray.clear();
  iActiveUnitArray.clear();
  SetLastElementType(KTypeUnknown);
}
//*****************************************************************************

void CUnitData::Print(const int l) const
{
  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
        const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
        dPrint(l,"TTimestamp: s=%d, us=%d", 
               ts->timestamp_s,
               ts->timestamp_us);
        break;
      }
        
      case KTypeCommand: {
        const TCommand *ts = 
          reinterpret_cast<const TCommand *>(b->data);
        dPrint(l,"TCommand: cmd = %x", 
               ts->cmd);
        break;
      }

        
      case KTypeUnitId: {
        const TUnitId *unitid = reinterpret_cast<const TUnitId *>(b->data);
        dPrint(l,"TUnitId: available_id: %u, active_id: %u, instance: '%s'",
               unitid->available_id,
               unitid->active_id,
               unitid->instance);
        break;
      }

      case KTypeUnitPose: {
        const TUnitPose *ptr = reinterpret_cast<const TUnitPose *>(b->data);
        dPrint(l,"TUnitPose: x: %f, y: %f, z: %f, xrot: %f, yrot: %f, zrot: %f",
               ptr->x_pos,
               ptr->y_pos,
               ptr->z_pos,
               ptr->x_angle,
               ptr->y_angle,
               ptr->z_angle);
        break;
      }
        
      case KTypeAvailableUnit: {
        const TAvailableUnit *ptr = reinterpret_cast<const TAvailableUnit *>(b->data);
        dPrint(l,"TAvailableUnit: available_id: %x, unit_type: %x, implementation: '%s'",
               ptr->available_id,
               ptr->unit_type,
               ptr->implementation);
        break;
      }
        
      case KTypeActiveUnit: {
        const TActiveUnit *ptr = reinterpret_cast<const TActiveUnit *>(b->data);
        dPrint(l,"TActiveUnit: active_id: %x, available_id: %x, instance: '%s'",
               ptr->active_id,
               ptr->available_id,
               ptr->instance);
        break;
      }
        
      default:
        // Unknown Type in BinBag. Do nothing.
        dPrint(10,"Unknown type %u, ignoring.", b->type);
        break;
      }
    }
  }
}
//*****************************************************************************

bool CUnitData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    ADDELEMENT(Common::TTimestamp, KTypeTimestamp, iTimestampPtr, aStamp);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CUnitData::SetCommand(const TCommand &aCmd)
{  
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCmd);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CUnitData::SetUnitId(const TUnitId &aUnitId)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iUnitIdPtr == NULL) {
    ADDELEMENT(TUnitId, KTypeUnitId, iUnitIdPtr, aUnitId);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CUnitData::SetUnitPose(const TUnitPose &aUnitPose)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iUnitPosePtr == NULL) {
    ADDELEMENT(TUnitPose, KTypeUnitPose, iUnitPosePtr, aUnitPose);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CUnitData::AddAvailableUnit(const TAvailableUnit &aAvailableUnit)
{
  bool result = false;
  if (!IsReadOnlyContainer() && 
      ( iAvailableUnitArray.empty() || GetLastElementType() == KTypeAvailableUnit) ) {
    // Array must be empty; OR the last element must have been
    // AvailableUnit (Must be sequentical)
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeAvailableUnit,
                                  (const char *)&aAvailableUnit, 
                                  sizeof(TAvailableUnit));
    
    // Push to array
    iAvailableUnitArray.push_back(reinterpret_cast<const TAvailableUnit *>(b->data));
    
    // Mark last element.
    SetLastElementType(KTypeAvailableUnit);

    // ok.
    result = true;
  }
  

  return result;
}
//*****************************************************************************

bool CUnitData::AddActiveUnit(const TActiveUnit &aActiveUnit)
{ 
  bool result = false;
  if (!IsReadOnlyContainer() && 
      ( iActiveUnitArray.empty() || GetLastElementType() == KTypeActiveUnit) ) {
    // Array must be empty; OR the last element must have been
    // ActiveUnit (Must be sequentical)
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeActiveUnit,
                                  (const char *)&aActiveUnit, 
                                  sizeof(TActiveUnit));
    
    // Push to array
    iActiveUnitArray.push_back(reinterpret_cast<const TActiveUnit *>(b->data));
    
    // Mark last element.
    SetLastElementType(KTypeActiveUnit);

    // ok.
    result = true;
  }
  

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

const MaCI::Common::TTimestamp *CUnitData::GetTimestamp(void) const
{
  return iTimestampPtr;
}
//*****************************************************************************

const TCommand *CUnitData::GetCommand(void) const
{
  return iCommandPtr;
}
//*****************************************************************************

const TUnitId *CUnitData::GetUnitId(void) const
{
  return iUnitIdPtr;
}
//*****************************************************************************

const TUnitPose *CUnitData::GetUnitPose(void) const
{
  return iUnitPosePtr;
}
//*****************************************************************************

const TAvailableUnitPtrArray &CUnitData::GetAvailableUnitArray(void) const
{
  return iAvailableUnitArray;
}
//*****************************************************************************

const TActiveUnitPtrArray &CUnitData::GetActiveUnitArray(void) const
{
  return iActiveUnitArray;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_UNITDATA_STANDALONE_MAIN
#include <string>

int main(void)
{
  using namespace MaCI::Unit;
  gim::binbag::CBinBag bb;
  CUnitData A;
  CUnitData B;
  bool r;

  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  
  printf("Encode: Timestamp(1,2) ... ");
  r = A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));
  assert(r == true);
  printf("OK.\n");


  printf("Encode: UnitId() ... ");
  r = A.SetUnitId(TUnitId(7, 7, "J2B2"));
  assert(r == true);
  printf("OK.\n");

  printf("Encode: UnitPose() ... ");
  r = A.SetUnitPose(TUnitPose(1.0, 2.0, 3.0, 4.0, 5.0, 6.0));
  assert(r == true);
  printf("OK.\n");


  printf("Encode: AddAvailableUnit() x 5 ... ");
  r = A.AddAvailableUnit(TAvailableUnit(7,KUnitTypeMachine, "J2B2"));
  assert(r == true);
  r = A.AddAvailableUnit(TAvailableUnit(7,KUnitTypeMachine, "Skywalker"));
  assert(r == true);
  r = A.AddAvailableUnit(TAvailableUnit(7,KUnitTypeMachine, "Marcu"));
  assert(r == true);
  r = A.AddAvailableUnit(TAvailableUnit(7,KUnitTypeMachine, "Landmine"));
  assert(r == true);
  r = A.AddAvailableUnit(TAvailableUnit(7,KUnitTypeMachine, "MottiPbV"));
  assert(r == true);
  printf("OK.\n");


  printf("Encode: AddActiveUnit() x 5 ... ");
  r = A.AddActiveUnit(TActiveUnit(1, 7, "First"));
  assert(r == true);
  r = A.AddActiveUnit(TActiveUnit(2, 7, "Second"));
  assert(r == true);
  r = A.AddActiveUnit(TActiveUnit(3, 7, "Third"));
  assert(r == true);
  r = A.AddActiveUnit(TActiveUnit(4, 7, "Fourth"));
  assert(r == true);
  r = A.AddActiveUnit(TActiveUnit(5, 7, "Fifth"));
  assert(r == true);

  printf("OK.\n");


  /**
   * \todo Add test sections for TUnitEvent and TCommand
   */
  //////////////////////////////////////////////////////////
  printf("Decoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: GetTimestamp() = (%d, %d) ... ", 
         B.GetTimestamp()->timestamp_s, 
         B.GetTimestamp()->timestamp_us);
  assert(B.GetTimestamp()->timestamp_s == 1);
  assert(B.GetTimestamp()->timestamp_us == 2);
  printf("Assertions OK.\n\n");

  
  printf("Decode: GetUnitId() = ");
  const TUnitId *unid = B.GetUnitId();
  assert(unid != NULL);
  printf("id: %u, name: '%s'\n",
         unid->available_id,
         unid->instance);
  printf("OK.\n");


  printf("Decode: GetUnitPose() = ");
  const TUnitPose *unip = B.GetUnitPose();
  assert(unip != NULL);
  printf("x: %f, y: %f, z: %f, xrot: %f, yrot: %f, zrot: %f\n",
         unip->x_pos,
         unip->y_pos,
         unip->z_pos,
         unip->x_angle,
         unip->y_angle,
         unip->z_angle);
  printf("OK.\n");


  TAvailableUnitPtrArray avua = B.GetAvailableUnitArray();
  assert(avua.size() == 5);
  for(EACH_IN_i(avua)) {
    const TAvailableUnit *au = *i;
    assert(au != NULL);
    printf("AvailableUnit: available_id: %u, unit_type: %u, implementation: '%s'\n",
           au->available_id,
           au->unit_type,
           au->implementation);
  }
  printf("OK.\n");
  

  TActiveUnitPtrArray acua = B.GetActiveUnitArray();
  assert(acua.size() == 5);
  for(EACH_IN_i(acua)) {
    const TActiveUnit *au = *i;
    printf("ActiveUnit: active_id: %u, available_id: %u, instance: '%s'\n",
           au->active_id,
           au->available_id,
           au->instance);
  }
  printf("OK.\n");


  /**
   * \todo Add test sections for TUnitEvent and TCommand
   */

  printf("Test complete.\n");
  

}
#endif
//*****************************************************************************
//*****************************************************************************
